/*
 * pid.h
 *
 *  Created on: 2021-09-27
 *      Author: chenss
 */

#ifndef INC_PID_H_
#define INC_PID_H_

#include "main.h"
#include "tick.h"
#include "arm_math.h"

typedef enum PID_Type
{
    PID_Type_Positional,
    PID_Type_Incremental,

    PID_Type_Unknown,
    PID_Type_Max
}PID_Type_t;

typedef struct PID_Positional
{
    float P;     //!< Proportional gain
    float I;     //!< Integral gain
    float D;     //!< Derivative gain

    float integral;     //!< last integral component value
    float prevError;        //!< last tracking error value
    Tick_t prevTimestamp;   //!< Last execution timestamp
    float prevOutput;       //!< last PID output value
} PID_Positional_t;

typedef struct PID_Incremental
{
    arm_pid_instance_f32 arm_pid;
    float prevOutput;       //!< last PID output value
} PID_Incremental_t;

typedef struct
{
    PID_Type_t type;
    float lowerLimit;
    float upperLimit;
    float ramp;  //!< Maximum speed of change of the output value

    union
    {
        PID_Positional_t positional;
        PID_Incremental_t incremental;
    };
}PID_t;

void PID_Init(PID_t *pid, PID_Type_t type, float P, float I, float D);
void PID_Set(PID_t *pid, float P, float I, float D);
void PID_Get(PID_t *pid, float *pP, float *pI, float *pD);
void PID_SetLimit(PID_t *pid, float lower, float upper);
void PID_SetRamp(PID_t *pid, float ramp);
float PID_Operate(PID_t *pid, float error);
void PID_Reset(PID_t *pid);
float PID_Output(PID_t *pid);

void PID_Positional_Init(PID_Positional_t *pid, float P, float I, float D);
float PID_Positional_Operate(PID_Positional_t *pid, float error, float lowerLimit, float upperLimit, float ramp);
void PID_Positional_Reset(PID_Positional_t *pid);

void PID_Incremental_Init(PID_Incremental_t *pid, float P, float I, float D);
float PID_Incremental_Operate(PID_Incremental_t *pid, float error, float lowerLimit, float upperLimit, float ramp);
void PID_Incremental_Reset(PID_Incremental_t *pid);

#endif /* INC_PID_H_ */
